using UnityEngine;
using System.Collections;

namespace DanielSig.Geometry.Orbiter
{
	public class Orbiter3D : Orbiter
	{
		public static explicit operator Vector2 (Orbiter3D orbiter)
		{
			return (Vector2)orbiter.direction;
		}
		public static explicit operator Vector3 (Orbiter3D orbiter)
		{
			return orbiter.direction;
		}
		
		public static Orbiter3D operator ++ (Orbiter3D orbiter)
		{
			orbiter.NextAndUpdate ();
			return orbiter;
		}
		public static Orbiter3D operator + (Orbiter3D orbiter, int iterations)
		{
			orbiter = orbiter.Copy () as Orbiter3D;
			if (iterations > 0) {
				for (int i = 0; i < iterations; i++) {
					orbiter.QuickNext ();
				}
			} else {
				for (int i = 0; i > iterations; i--) {
					orbiter.QuickPrevious ();
				}
			}
			orbiter._iterations += iterations;
			orbiter.Update ();
			return orbiter;
		}
		
		public new Vector3 direction {
			get {
				_direction.x = (float)_dirX;
				_direction.y = (float)_dirY;
				_direction.z = (float)_dirZ;
				return _direction;
			}
		}
		public virtual float z {
			get { return (float)_dirZ; }
		}
		public virtual double zDouble {
			get { return _dirZ; }
		}
		
		private Orbiter3D (float angleSpeed, double gravity, double velX, double velY, double velZ, double dirX, double dirY, double dirZ, int iterations)
		{
			_angleSpeed = angleSpeed;
			_gravity = gravity;
			_velX = velX;
			_velY = velY;
			_velZ = velZ;
			_dirX = dirX;
			_dirY = dirY;
			_dirZ = dirZ;
			_iterations = iterations;
		}
	
		public Orbiter3D ()
		{
			_direction = Vector3.up;
			_axis = Vector3.forward;
			InitOrbiter ();
		}
		public Orbiter3D (float angleSpeed, Vector3 axis)
		{
			_direction = Vector3.up;
			_angleSpeed = angleSpeed;
			_axis = axis;
			InitOrbiter ();
		}
		public Orbiter3D (Vector3 direction, float angleSpeed)
		{
			_direction = direction;
			_angleSpeed = angleSpeed;
			_axis = Vector3.forward;
			InitOrbiter ();
		}
		public Orbiter3D (Vector3 direction, float angleSpeed, Vector3 axis)
		{
			_direction = direction;
			_angleSpeed = angleSpeed;
			_axis = axis;
			InitOrbiter ();
		}
		public Orbiter3D (float dirX, float dirY, float dirZ, float angleSpeed, float axisX, float axisY, float axisZ)
		{
			_direction = new Vector3 (dirX, dirY, dirZ);
			_angleSpeed = angleSpeed;
			_axis = new Vector3 (axisX, axisY, axisZ);
			InitOrbiter ();
		}
		
		public override Orbiter Copy ()
		{
			return new Orbiter3D (_angleSpeed, _gravity, _velX, _velY, _velZ, _dirX, _dirY, _dirZ, _iterations) as Orbiter;
		}
		public override void Next ()
		{
			_velX -= _dirX * _gravity;
			_velY -= _dirY * _gravity;
			_velZ -= _dirZ * _gravity;
			_dirX = _dirX + _velX;
			_dirY = _dirY + _velY;
			_dirZ = _dirZ + _velZ;
			_iterations++;
		}
		public override void Previous ()
		{
			_dirX = _dirX - _velX;
			_dirY = _dirY - _velY;
			_dirZ = _dirZ - _velZ;
			_velX += _dirX * _gravity;
			_velY += _dirY * _gravity;
			_velZ += _dirZ * _gravity;
			_iterations--;
		}
		public override void Update ()
		{
			_direction.x = (float)_dirX;
			_direction.y = (float)_dirY;
			_direction.z = (float)_dirZ;
		}
		public override void NextAndUpdate ()
		{
			_velX -= _dirX * _gravity;
			_velY -= _dirY * _gravity;
			_velZ -= _dirZ * _gravity;
			_dirX = _dirX + _velX;
			_dirY = _dirY + _velY;
			_dirZ = _dirZ + _velZ;
			_direction.x = (float)_dirX;
			_direction.y = (float)_dirY;
			_direction.z = (float)_dirZ;
			_iterations++;
		}
		public override void PreviousAndUpdate ()
		{
			_dirX = _dirX - _velX;
			_dirY = _dirY - _velY;
			_dirZ = _dirZ - _velZ;
			_velX += _dirX * _gravity;
			_velY += _dirY * _gravity;
			_velZ += _dirZ * _gravity;
			_direction.x = (float)_dirX;
			_direction.y = (float)_dirY;
			_direction.z = (float)_dirZ;
			_iterations--;
		}
	
		protected Vector3 _direction;
		protected Vector3 _axis;
		
		protected double _velZ;
		protected double _dirZ;
		
		protected override void QuickNext ()
		{
			_velX -= _dirX * _gravity;
			_velY -= _dirY * _gravity;
			_velZ -= _dirZ * _gravity;
			_dirX = _dirX + _velX;
			_dirY = _dirY + _velY;
			_dirZ = _dirZ + _velZ;
		}
		protected override void QuickPrevious ()
		{
			_dirX = _dirX - _velX;
			_dirY = _dirY - _velY;
			_dirZ = _dirZ - _velZ;
			_velX += _dirX * _gravity;
			_velY += _dirY * _gravity;
			_velZ += _dirZ * _gravity;
		}
	
		protected override void InitOrbiter ()
		{
			base.InitOrbiter ();
			
			_dirX = _direction.x;
			_dirY = _direction.y;
			_dirZ = _direction.z;
			
			Vector3 prevDir = Quaternion.AngleAxis (-_angleSpeed, _axis) * _direction;
			
			Vector3 velocity = _direction - prevDir;
			_velX = velocity.x;
			_velY = velocity.y;
			_velZ = velocity.z;
			
			_gravity = (velocity - Vector3.Reflect(velocity, _direction.normalized)).magnitude / _direction.magnitude;
		}
	}
}